// Copyright (c) 2010 Omer Goshen
//
// Permission is hereby granted, free of charge, to any person obtaining
// a copy of this software and associated documentation files (the
// "Software"), to deal in the Software without restriction, including
// without limitation the rights to use, copy, modify, merge, publish,
// distribute, sublicense, and/or sell copies of the Software, and to
// permit persons to whom the Software is furnished to do so, subject to
// the following conditions:
//
// The above copyright notice and this permission notice shall be
// included in all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
// LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
// WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

#include "Car.h"

using namespace std;
using namespace Ogre;
using namespace BtOgre;

void Car::init(Ogre::Root* ogreRoot, SceneManager* sceneMgr, btDiscreteDynamicsWorld* phyWorld, String xmlFile)
{
	Car::init(ogreRoot, sceneMgr);

	this->phyWorld = phyWorld;
	this->mSceneMgr = sceneMgr;


	TiXmlDocument xml;
	xml.LoadFile(xmlFile.c_str());
	TiXmlElement* root = xml.RootElement();

	BtOgre::StaticMeshToShapeConverter converter(mEntity);
//	mCollisionShape = converter.createConvex();
//	mCollisionShape = converter.createBox();
	btCompoundShape* compound = new btCompoundShape();
	btTransform localTrans;
	localTrans.setIdentity();
	//localTrans effectively shifts the center of mass with respect to the chassis
	//localTrans.setOrigin(btVector3(0,0.5,-1.5));
	//localTrans.setOrigin(btVector3(0,0.5,0));

//	btCollisionShape* chassisShape = new btBoxShape(btVector3(2.6f,1.0f, 7.f));
//	btCollisionShape* chassisShape =  converter.createBox();
	btCollisionShape* chassisShape =  converter.createConvex();

	compound->addChildShape(localTrans,chassisShape);
	mCollisionShape = compound;

	//Calculate inertia.
	mass = StringConverter::parseReal(root->Attribute("mass"));
	btVector3 inertia;

	mCollisionShape->calculateLocalInertia(mass, inertia);

	btTransform trans;
	trans.setOrigin(BtOgre::Convert::toBullet(mNode->getPosition()));
	trans.setRotation(BtOgre::Convert::toBullet(mNode->getOrientation()));
	btMotionState* state = new btDefaultMotionState(trans);

	btRigidBody::btRigidBodyConstructionInfo CI(mass, state, mCollisionShape, inertia);
//	CI.m_restitution = 0.5;
//	CI.m_friction = 0.8;
	mRigidBody = new btRigidBody(CI);

	mRigidBody->setActivationState(DISABLE_DEACTIVATION);
	mRigidBody->setCollisionFlags(mRigidBody->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);

//	mRigidBody->setDamping(0.0f, 0.5f);
	mRigidBody->setDamping(0.05f, 0.4f);
//    phyWorld->addRigidBody(mRigidBody, COL_BIKE, COL_ACTOR | COL_WALL | COL_LASER);
	phyWorld->addRigidBody(mRigidBody);

	engineMax = StringConverter::parseReal(root->FirstChildElement("engine")->Attribute("max"));
	brakeForce = StringConverter::parseReal(root->FirstChildElement("brakes")->Attribute("force"));

	vehicleRayCaster = new btDefaultVehicleRaycaster(phyWorld);

	vehicle = new btRaycastVehicle(tuning, mRigidBody,  vehicleRayCaster);
	vehicle->setCoordinateSystem(0,1,2);


	TiXmlElement* suspension = root->FirstChildElement("suspension");

	btVector3 wheelDir(0,-1,0);
	btVector3 wheelAxel(-1,0,0);
	btScalar suspensionRestLength(StringConverter::parseReal(suspension->Attribute("restLength")));

	TiXmlElement* wheel = root->FirstChildElement("wheel");

	int i = 0;
	do {
			cout << wheel << endl;

			Vector3 pos = StringConverter::parseVector3(wheel->Attribute("pos"));
			Real radius = StringConverter::parseReal(wheel->Attribute("radius"));
			String mesh = wheel->Attribute("mesh");
			bool isFront = wheel->Attribute("front");

			Entity* ent = sceneMgr->createEntity("Wheel"+i, mesh);
			SceneNode* node = mNode->createChildSceneNode(pos-Vector3(0,suspensionRestLength,0));
			if (i%2==0)
				node->rotate(Vector3::UNIT_Y, Degree(180), Node::TS_WORLD);
			node->attachObject(ent);

			wheelNodes.push_back(node);


			vehicle->addWheel(
			    Convert::toBullet(pos),
			    wheelDir,
			    wheelAxel,
			    suspensionRestLength,
			    radius,
			    tuning,
			    isFront);
			i++;
		} while ((wheel = wheel->NextSiblingElement()));

	phyWorld->addVehicle(vehicle);

	float	wheelFriction = 100000; //BT_LARGE_FLOAT;
	float	suspensionStiffness = StringConverter::parseReal(suspension->Attribute("stiffness"));
	float	suspensionDamping = StringConverter::parseReal(suspension->Attribute("damping"));
	float	suspensionCompression = StringConverter::parseReal(suspension->Attribute("compression"));
	float	rollInfluence = 0.1f;


	for (int i=0;i<vehicle->getNumWheels();i++) {
			btWheelInfo& wheel = vehicle->getWheelInfo(i);
			wheel.m_suspensionStiffness = suspensionStiffness;
			wheel.m_wheelsDampingRelaxation = suspensionDamping;
			wheel.m_wheelsDampingCompression = suspensionCompression;
			wheel.m_frictionSlip = wheelFriction;
			wheel.m_rollInfluence = rollInfluence;
		}

	vehicle->resetSuspension();
	for (int i=0;i<vehicle->getNumWheels();i++) {
			//synchronize the wheels with the (interpolated) chassis worldtransform
			vehicle->updateWheelTransform(i,true);
		}

//	wheelParticles = mSceneMgr->createParticleSystem("WheelParticles", "Dust");
//	SceneNode* particleNode = mNode->createChildSceneNode("WheelParticleNode");
//	particleNode->attachObject(wheelParticles);
//	//particleNode->translate(0,.2,-4.0);
//	wheelParticles->setCastShadows(true);

}


void Car::init(Ogre::Root* OgreRoot, SceneManager* sceneMgr)
{

	Actor::init(OgreRoot);

//    if (!OgreRoot->getSceneManagerIterator().hasMoreElements())
//        OGRE_EXCEPT( Exception::ERR_INTERNAL_ERROR, "No scene manager found!", "init" );

//    SceneManager* sceneMgr = OgreRoot->getSceneManagerIterator().getNext();
	mSceneMgr = sceneMgr;

	/// Lights
	Light* headLight = sceneMgr->createLight("HeadLight1");
	headLight->setPosition(2.0, .85, 4.0);
	headLight->setDirection(0,-0.2,1);
	headLight->setType(Light::LT_SPOTLIGHT);
	headLight->setDiffuseColour(ColourValue(1.0f, 1.0f, 1.0f));
	headLight->setSpecularColour(ColourValue(1.0f, 1.0f, 1.0f));
//	headLight->setPowerScale(10.f);
	headLight->setSpotlightFalloff(100.0f);
	headLight->setCastShadows(true);
//	headLight->setShadowFarDistance(50.0f);
	mNode->attachObject(headLight);

	headLight = sceneMgr->createLight("HeadLight2");
	headLight->setPosition(-2.0, .85, 4.0);
	headLight->setDirection(0,-0.2,1);
	headLight->setType(Light::LT_SPOTLIGHT);
	headLight->setDiffuseColour(ColourValue(1.0f, 1.0f, 1.0f));
	headLight->setSpecularColour(ColourValue(1.0f, 1.0f, 1.0f));
//	headLight->setPowerScale(10.f);
	headLight->setSpotlightFalloff(100.0f);	
	headLight->setCastShadows(true);
//	headLight->setShadowFarDistance(50.0f);
	mNode->attachObject(headLight);
}

Car::Car(SceneManager* sceneMgr) :
		Actor(sceneMgr, "Car"),
		engineMax(2000.0f),
		brakeForce(500.0f),
		throttle(.0),
		steering(.0),
		brakes(.0),
		engineIncrement(.0),
		steeringIncrement(.0),
		brakeIncrement(.0),
		steerLimit(0.7f)
{
	wheelParticles = 0;
}


Car::~Car()
{
	delete mEntity;
	delete mNode;
	delete mRigidBody;
	delete vehicleRayCaster;
	delete vehicle;
}



bool Car::frameStarted(const Ogre::FrameEvent &evt)
{
	Real dt = evt.timeSinceLastFrame;

//    mRigidBody->setActivationState(DISABLE_DEACTIVATION);
	if (steeringIncrement==0)
		//steering *= 0.9f;
		steering *= 1 - exp(-3);
	else
		steering += steeringIncrement * dt;

//    // Limit steering
	steering = Math::Clamp(steering, -steerLimit, steerLimit);

	throttle += (engineIncrement>0 ? exp(-throttle) : -2.0*(exp(throttle)-exp(-throttle))) * dt;
	throttle = Math::Clamp(throttle, -1.f, 1.f);

	brakes += (brakeIncrement>0?exp(-brakes*dt)*dt:0);
	brakes = Math::Clamp(brakes, 0.f, 1.f);

	return true;
}


bool Car::frameEnded(const Ogre::FrameEvent &evt)
{
	vehicle->setSteeringValue(steering,0);
	vehicle->setSteeringValue(steering,1);

	vehicle->applyEngineForce(engineMax * throttle, 0);
	vehicle->applyEngineForce(engineMax * throttle, 1);

	vehicle->setBrake(brakeForce * brakes,2);
	vehicle->setBrake(brakeForce * brakes,3);

	for (int i=0;i<vehicle->getNumWheels();i++) {
//			//synchronize the wheels with the (interpolated) chassis worldtransform
			vehicle->updateWheelTransform(i,true);
			btWheelInfo wi = vehicle->getWheelInfo(i);
			SceneNode* node = wheelNodes[i];

//			wheelEmitters.push_back(emitter);

			Quaternion q;
			Quaternion q1;
			Quaternion q2;
			if(wi.m_bIsFrontWheel)
			q.FromAngleAxis(Radian(wi.m_steering), Vector3::UNIT_Y);
			q1.FromAngleAxis(Degree(i%2==0?180:0), Vector3::UNIT_Y);
			q2.FromAngleAxis(Radian(wi.m_rotation*(i%2==0?-1:1)), Vector3::UNIT_X);

			node->setOrientation(q*q1*q2);

			btVector3 cp = wi.m_raycastInfo.m_contactPointWS;

//			if(wi.m_raycastInfo.m_isInContact)
//			{
//				ParticleEmitter* emit = wheelParticles->getEmitter(i);
//				emit->setEmissionRate((1-wi.m_skidInfo)*(wi.m_deltaRotation)*3000);
//			}

		}

	btTransform trans = mRigidBody->getWorldTransform();

	mNode->setPosition(BtOgre::Convert::toOgre(trans.getOrigin()));
	mNode->setOrientation(BtOgre::Convert::toOgre(trans.getRotation()));


	return true;
}
